﻿/* ***************************************************************************
 *  This file is part of the C# port of the Invivo AI challenge simulation environment.
 *  Copyright 2010 Colin Green
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program .  If not, see <http://www.gnu.org/licenses/>.
 * 
 * 
 * This file incorporates open sourced work authored by the following people:
 *
 *   Copyright (c) 2009 Simon Funk
 */
using Box2DX.Common;
using Box2DX.Dynamics;
using System;

namespace FunkAIChallenge
{
    /// <summary>
    /// Represents a player arm. Properties are exposed to obtain arm state and SetJointTorque() 
    /// allows control of the arm joints.
    /// </summary>
    public class Player
    {
        #region Consts
        const float __jointMaxSpeed = (float)(System.Math.PI * 4.0);
        const float __jointFriction = 0.5f;
        const float __PiHalf = (float)(System.Math.PI / 2.0);
        #endregion

        #region Instance Fields

        Vec2 _playerBasePos;
        Vec2 _armBasePos;
        Body _handBody;
        RevoluteJoint[] _jointArr;
        int _jointCount;

        float[] _jointAngleArr;
        float[] _jointVelocityArr;

        #endregion

        #region Constructor

        /// <summary>
        /// Constructs with the provided hand object and array of player arm joint objects.
        /// </summary>
        public Player(Vec2 playerBasePosition, Vec2 armBasePosition, Body handBody, RevoluteJoint[] jointArr)
        {
            if(3 != jointArr.Length) {
                throw new ApplicationException("Unexpected number of player arm joints.");
            }

            _playerBasePos = playerBasePosition;
            _armBasePos = armBasePosition;
            _handBody = handBody;
            _jointArr = jointArr;
            _jointCount = _jointArr.Length;
            _jointAngleArr = new float[_jointCount];
            _jointVelocityArr = new float[_jointCount];
        }

        #endregion

        #region Public [Get State]

        /// <summary>
        /// Position of the player arm's base (base center of mass).
        /// </summary>
        public Vec2 BasePosition
        {
            get { return _playerBasePos; }
        }

        /// <summary>
        /// Position of the player arm's first joint (attached to the top of the player base
        /// and thus slightly above BasePosition.
        /// </summary>
        public Vec2 ArmBasePosition
        {
            get { return _armBasePos; }
        }

        /// <summary>
        /// Gets the hand body object.
        /// </summary>
        public Body HandBody
        {
            get { return _handBody; }
        }

        /// <summary>
        /// Update the internally held array of joint angles and return it.
        /// </summary>
        public float[] GetJointAngleArray()
        {
            // Update the joint angle array.
            // Compensate for different origin angles (make zero angle point arm straight up). 
            // Note. The python code does this same correction.
            _jointAngleArr[0] = _jointArr[0].JointAngle;
            _jointAngleArr[1] = _jointArr[1].JointAngle - __PiHalf;
            _jointAngleArr[2] = _jointArr[2].JointAngle + __PiHalf;
            return _jointAngleArr;
        }

        /// <summary>
        /// Update the internally held array of joint velocities and return it.
        /// </summary>
        public float[] GetJointVelocityArray()
        {
            // Update the joint angle array.
            for(int i=0; i<_jointCount; i++) {
                _jointVelocityArr[i] = _jointArr[i].JointSpeed;
            }
            return _jointVelocityArr;
        }

        #endregion

        #region Public [Control]

        /// <summary>
        /// Sets the torque of the specified joint.
        /// </summary>
        public void SetJointTorque(int idx, float torque)
        {
            float atorque; // abs(torque)
            float storque; // sign(torque)
		    if(torque > 0f) {
			    atorque = torque;	
			    storque = 1f;		
            } else if (torque < 0f) {
                atorque = -torque;
                storque = -1f;
            } else {
                atorque = 0f;
                storque = 0f;
            }

            //#
            //# We have to enumerate various cases because we are emulating friction
            //#  with active torque and we have to take care not to add energy to
            //#  the system when a reversal happens inside the timestep.
            //#
            //# Also, the underlying sim does not let us apply torque directly, but
            //#  rather we have to factor it into maxMotorTorque and motorSpeed, the
            //#  latter of which we set to an extreme we don't normally expect to reach.
            //#
            RevoluteJoint joint = _jointArr[idx];
            float speed = joint.JointSpeed;
            float fric = __jointFriction;

		    if(speed*storque >= 0f) //# Pushing in same direction as current motion (and against friction)
            {
                if(atorque > fric) {    //#     With enough force to overcome friction:
                    joint.SetMaxMotorTorque(atorque	- fric);    //# Remaining torque (after friction)...
                    joint.MotorSpeed = storque * 100f;          //# serves to accelerate current motion
                } else {                //#     Without enough force to overcome friction:
                    joint.SetMaxMotorTorque(fric - atorque);    //# Remaining friction...
                    joint.MotorSpeed = 0f;                      //# ...serves to stop the motion (but no more)
                }
            }
            else {                  //# Pushing against current motion (and with friction)
                joint.SetMaxMotorTorque(fric + atorque);    //# Forces sum...
                joint.MotorSpeed = storque * 100f;          //# ...in direction of torque
                //# BUG: This case unfortunately adds energy during reversals because friction
                //#  continues to contribute energy in the same direction after the reversal.
                //# Is there a fix for this?
	        }						
        }

        #endregion
    }
}
